/*
  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

  This software is provided 'as-is', without any express or implied
  warranty.  In no event will the authors be held liable for any damages
  arising from the use of this software.

  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:

  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
*/

using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.BulletX
{
	[Serializable]
	public class UsageBitfield
	{
		public bool UsedVertexA, UsedVertexB, UsedVertexC, UsedVertexD;

		public void Reset()
		{
			UsedVertexA = UsedVertexB = UsedVertexC = UsedVertexD = false;
		}
	}

	[Serializable]
	public class SubSimplexClosestResult
	{
		public Vector3 ClosestPointOnSimplex;

		//MASK for mUsedVertices
		//stores the simplex vertex-usage, using the MASK, 
		// if mUsedVertices & MASK then the related vertex is used
		public UsageBitfield UsedVertices = new UsageBitfield();
		public float[] BarycentricCoords = new float[ 4 ];
		public bool Degenerate;

		public void Reset()
		{
			Degenerate = false;
			BarycentricCoords[ 0 ] = BarycentricCoords[ 1 ] = BarycentricCoords[ 2 ] = BarycentricCoords[ 3 ] = 0.0f;
			UsedVertices.Reset();
		}

		public bool IsValid
		{
			get
			{
				return (BarycentricCoords[0] >= 0f) &&
						(BarycentricCoords[1] >= 0f) &&
						(BarycentricCoords[2] >= 0f) &&
						(BarycentricCoords[3] >= 0f);
			}
		}

		public void SetBarycentricCoordinates(float a, float b, float c, float d)
		{
			BarycentricCoords[0] = a;
			BarycentricCoords[1] = b;
			BarycentricCoords[2] = c;
			BarycentricCoords[3] = d;
		}
	}

	/// VoronoiSimplexSolver is an implementation of the closest point distance
	/// algorithm from a 1-4 points simplex to the origin.
	/// Can be used with GJK, as an alternative to Johnson distance algorithm. 
	[Serializable]
	public class VoronoiSimplexSolver : ISimplexSolver
	{
		private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3;

		private const int VoronoiSimplexMaxVerts = 5;
		private const bool CatchDegenerateTetrahedron = true;

		private int _numVertices;

		private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts];
		private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts];
		private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts];

		private Vector3 _cachedPA;
		private Vector3 _cachedPB;
		private Vector3 _cachedV;
		private Vector3 _lastW;
		private bool _cachedValidClosest;

		private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult();
		private SubSimplexClosestResult _tempResult = new SubSimplexClosestResult();

		private bool _needsUpdate;
	
		#region ISimplexSolver Members

        public bool FullSimplex
		{
			get
			{
				return _numVertices == 4;
			}
		}

		public int NumVertices { get { return _numVertices; } }

		public void Reset()
		{
 			_cachedValidClosest = false;
			_numVertices = 0;
			_needsUpdate = true;
			_lastW = new Vector3(1e30f, 1e30f, 1e30f);
			_cachedBC.Reset();
		}

        public void AddVertex(Vector3 w, Vector3 p, Vector3 q)
		{
 			_lastW = w;
			_needsUpdate = true;

			_simplexVectorW[_numVertices] = w;
			_simplexPointsP[_numVertices] = p;
			_simplexPointsQ[_numVertices] = q;

			_numVertices++;
		}

		//return/calculate the closest vertex
        public bool Closest(out Vector3 v)
		{
 			bool succes = UpdateClosestVectorAndPoints();
			v = _cachedV;
			return succes;
		}

        public float MaxVertex
		{
			get
			{
				int numverts = _numVertices;
				float maxV = 0f, curLen2;
				for (int i = 0; i < numverts; i++)
				{
					curLen2 = _simplexVectorW[i].LengthSquared();
					if (maxV < curLen2) maxV = curLen2;
				}
				return maxV;
			}
		}

		//return the current simplex
        public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf)
		{
			int numverts = _numVertices;
			pBuf = new Vector3[numverts];
			qBuf = new Vector3[numverts];
			yBuf = new Vector3[numverts];
			for (int i = 0; i < numverts; i++)
			{
				yBuf[i] = _simplexVectorW[i];
				pBuf[i] = _simplexPointsP[i];
				qBuf[i] = _simplexPointsQ[i];
			}
			return numverts;
		}

		public bool InSimplex(Vector3 w)
		{
			//check in case lastW is already removed
			if (w == _lastW) return true;

			//w is in the current (reduced) simplex
			int numverts = _numVertices;
			for (int i = 0; i < numverts; i++)
				if (_simplexVectorW[i] == w) return true;
					    	
			return false;
		}

        public void BackupClosest(out Vector3 v)
		{
 			v = _cachedV;
		}

        public bool EmptySimplex
		{
			get
			{
				return _numVertices == 0;
			}
		}

        public void ComputePoints(out Vector3 p1, out Vector3 p2)
		{
 			UpdateClosestVectorAndPoints();
			p1 = _cachedPA;
			p2 = _cachedPB;
		}

		#endregion
		
		public void RemoveVertex(int index)
		{
            BulletDebug.Assert(_numVertices > 0);
			_numVertices--;
			_simplexVectorW[index] = _simplexVectorW[_numVertices];
			_simplexPointsP[index] = _simplexPointsP[_numVertices];
			_simplexPointsQ[index] = _simplexPointsQ[_numVertices];
		}

		public void ReduceVertices(UsageBitfield usedVerts)
		{
			if ((_numVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3);
			if ((_numVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2);
			if ((_numVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1);
			if ((_numVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0);
		}

		public bool UpdateClosestVectorAndPoints()
		{	
			if (_needsUpdate)
			{
				_cachedBC.Reset();
				_needsUpdate = false;

				Vector3 p, a, b, c, d;
				switch (_numVertices)
				{
					case 0:
						_cachedValidClosest = false;
						break;
					case 1:
						_cachedPA = _simplexPointsP[0];
						_cachedPB = _simplexPointsQ[0];
						_cachedV = _cachedPA - _cachedPB;
						_cachedBC.Reset();
						_cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f);
						_cachedValidClosest = _cachedBC.IsValid;
						break;
					case 2:
						//closest point origin from line segment
						Vector3 from = _simplexVectorW[0];
						Vector3 to = _simplexVectorW[1];
						Vector3 nearest;

						Vector3 diff = -from;
						Vector3 v = to - from;
						float t = Vector3.Dot(v, diff);
							
						if (t > 0)
						{
							float dotVV = v.LengthSquared();
							if (t < dotVV)
							{
								t /= dotVV;
								diff -= t * v;
								_cachedBC.UsedVertices.UsedVertexA = true;
								_cachedBC.UsedVertices.UsedVertexB = true;
							}
							else
							{
								t = 1;
								diff -= v;
								//reduce to 1 point
								_cachedBC.UsedVertices.UsedVertexB = true;
							}
						}
						else
						{
							t = 0;
							//reduce to 1 point
							_cachedBC.UsedVertices.UsedVertexA = true;
						}

						_cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);
						nearest = from + t * v;

						_cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
						_cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
						_cachedV = _cachedPA - _cachedPB;

						ReduceVertices(_cachedBC.UsedVertices);

						_cachedValidClosest = _cachedBC.IsValid;
						break;
					case 3:
						//closest point origin from triangle
						p = new Vector3();
						a = _simplexVectorW[0];
						b = _simplexVectorW[1];
						c = _simplexVectorW[2];

						ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);
						_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
										_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
										_simplexPointsP[2] * _cachedBC.BarycentricCoords[2];

						_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
										_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
										_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2];

						_cachedV = _cachedPA - _cachedPB;

						ReduceVertices(_cachedBC.UsedVertices);
						_cachedValidClosest = _cachedBC.IsValid;
						break;
					case 4:
						p = new Vector3();
						a = _simplexVectorW[0];
						b = _simplexVectorW[1];
						c = _simplexVectorW[2];
						d = _simplexVectorW[3];

						bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);

						if (hasSeperation)
						{
							_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
											_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
											_simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
											_simplexPointsP[3] * _cachedBC.BarycentricCoords[3];

							_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
											_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
											_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
											_simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];

							_cachedV = _cachedPA - _cachedPB;
							ReduceVertices(_cachedBC.UsedVertices);
						}
						else
						{
							if (_cachedBC.Degenerate)
							{
								_cachedValidClosest = false;
							}
							else
							{
								_cachedValidClosest = true;
								//degenerate case == false, penetration = true + zero
								_cachedV.X = _cachedV.Y = _cachedV.Z = 0f;
							}
							break; // !!!!!!!!!!!! proverit na vsakiy sluchai
						}

						_cachedValidClosest = _cachedBC.IsValid;

						//closest point origin from tetrahedron
						break;
					default:
						_cachedValidClosest = false;
						break;
				}
			}

			return _cachedValidClosest;
		}

		public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c,
			ref SubSimplexClosestResult result)
		{
			result.UsedVertices.Reset();

			float v, w;

			// Check if P in vertex region outside A
			Vector3 ab = b - a;
			Vector3 ac = c - a;
			Vector3 ap = p - a;
			float d1 = Vector3.Dot(ab, ap);
			float d2 = Vector3.Dot(ac, ap);
			if (d1 <= 0f && d2 <= 0f) 
			{
				result.ClosestPointOnSimplex = a;
				result.UsedVertices.UsedVertexA = true;
				result.SetBarycentricCoordinates(1, 0, 0, 0);
				return true; // a; // barycentric coordinates (1,0,0)
			}

			// Check if P in vertex region outside B
			Vector3 bp = p - b;
			float d3 = Vector3.Dot(ab, bp);
			float d4 = Vector3.Dot(ac, bp);
			if (d3 >= 0f && d4 <= d3) 
			{
				result.ClosestPointOnSimplex = b;
				result.UsedVertices.UsedVertexB = true;
				result.SetBarycentricCoordinates(0, 1, 0, 0);

				return true; // b; // barycentric coordinates (0,1,0)
			}
			// Check if P in edge region of AB, if so return projection of P onto AB
			float vc = d1 * d4 - d3 * d2;
			if (vc <= 0f && d1 >= 0f && d3 <= 0f)
			{
				v = d1 / (d1 - d3);
				result.ClosestPointOnSimplex = a + v * ab;
				result.UsedVertices.UsedVertexA = true;
				result.UsedVertices.UsedVertexB = true;
				result.SetBarycentricCoordinates(1 - v, v, 0, 0);
				return true;
				//return a + v * ab; // barycentric coordinates (1-v,v,0)
			}

			// Check if P in vertex region outside C
			Vector3 cp = p - c;
			float d5 = Vector3.Dot(ab, cp);
			float d6 = Vector3.Dot(ac, cp);
			if (d6 >= 0f && d5 <= d6) 
			{
				result.ClosestPointOnSimplex = c;
				result.UsedVertices.UsedVertexC = true;
				result.SetBarycentricCoordinates(0, 0, 1, 0);
				return true;//c; // barycentric coordinates (0,0,1)
			}

			// Check if P in edge region of AC, if so return projection of P onto AC
			float vb = d5 * d2 - d1 * d6;
			if (vb <= 0f && d2 >= 0f && d6 <= 0f)
			{
				w = d2 / (d2 - d6);
				result.ClosestPointOnSimplex = a + w * ac;
				result.UsedVertices.UsedVertexA = true;
				result.UsedVertices.UsedVertexC = true;
				result.SetBarycentricCoordinates(1 - w, 0, w, 0);
				return true;
				//return a + w * ac; // barycentric coordinates (1-w,0,w)
			}

			// Check if P in edge region of BC, if so return projection of P onto BC
			float va = d3 * d6 - d5 * d4;
			if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
			{
				w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

				result.ClosestPointOnSimplex = b + w * (c - b);
				result.UsedVertices.UsedVertexB = true;
				result.UsedVertices.UsedVertexC = true;
				result.SetBarycentricCoordinates(0, 1 - w, w, 0);
				return true;		
			   // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
			}

			// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
			float denom = 1.0f / (va + vb + vc);
			v = vb * denom;
			w = vc * denom;

			result.ClosestPointOnSimplex = a + ab * v + ac * w;
			result.UsedVertices.UsedVertexA = true;
			result.UsedVertices.UsedVertexB = true;
			result.UsedVertices.UsedVertexC = true;
			result.SetBarycentricCoordinates(1 - v - w, v, w, 0);
			
			return true;
		}

		/// Test if point p and d lie on opposite sides of plane through abc
		public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d)
		{
			Vector3 normal = Vector3.Cross(b - a, c - a);

			float signp = Vector3.Dot(p - a, normal); // [AP AB AC]
			float signd = Vector3.Dot(d - a, normal); // [AD AB AC]

			if (CatchDegenerateTetrahedron)
				if (signd * signd < (1e-4f * 1e-4f)) return -1;

			// Points on opposite sides if expression signs are opposite
			return signp * signd < 0f ? 1 : 0;
		}

		public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d,
			ref SubSimplexClosestResult finalResult)
		{
			// Start out assuming point inside all halfspaces, so closest to itself
			finalResult.ClosestPointOnSimplex = p;
			finalResult.UsedVertices.UsedVertexA = finalResult.UsedVertices.UsedVertexB = finalResult.UsedVertices.UsedVertexC = finalResult.UsedVertices.UsedVertexD = true;

			int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);
			int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);
  			int	pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);
			int	pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a);

			if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
			{
				finalResult.Degenerate = true;
				return false;
			}

			if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
				return false;

			float bestSqDist = float.MaxValue;
			// If point outside face abc then compute closest point on abc
			if (pointOutsideABC != 0) 
			{
				ClosestPtPointTriangle(p, a, b, c, ref _tempResult);
				Vector3 q = _tempResult.ClosestPointOnSimplex;

				float sqDist = ((Vector3)(q - p)).LengthSquared();
				// Update best closest point if (squared) distance is less than current best
				if (sqDist < bestSqDist)
				{
					bestSqDist = sqDist;
					finalResult.ClosestPointOnSimplex = q;
					//convert result bitmask!
					finalResult.UsedVertices.UsedVertexA = _tempResult.UsedVertices.UsedVertexA;
					finalResult.UsedVertices.UsedVertexB = _tempResult.UsedVertices.UsedVertexB;
					finalResult.UsedVertices.UsedVertexC = _tempResult.UsedVertices.UsedVertexC;
					finalResult.UsedVertices.UsedVertexD = false;
					finalResult.SetBarycentricCoordinates(
							_tempResult.BarycentricCoords[VertexA],
							_tempResult.BarycentricCoords[VertexB],
							_tempResult.BarycentricCoords[VertexC],
							0);
				}
			}
		  
			// Repeat test for face acd
			if (pointOutsideACD != 0) 
			{
				ClosestPtPointTriangle(p, a, c, d, ref _tempResult);
				Vector3 q = _tempResult.ClosestPointOnSimplex;
				//convert result bitmask!

				float sqDist = ((Vector3)(q - p)).LengthSquared();
				if (sqDist < bestSqDist) 
				{
					bestSqDist = sqDist;
					finalResult.ClosestPointOnSimplex = q;
					finalResult.UsedVertices.UsedVertexA = _tempResult.UsedVertices.UsedVertexA;
					finalResult.UsedVertices.UsedVertexC = _tempResult.UsedVertices.UsedVertexB;
					finalResult.UsedVertices.UsedVertexD = _tempResult.UsedVertices.UsedVertexC;
					finalResult.UsedVertices.UsedVertexB = false;
					finalResult.SetBarycentricCoordinates(
							_tempResult.BarycentricCoords[VertexA],
							0,
							_tempResult.BarycentricCoords[VertexB],
							_tempResult.BarycentricCoords[VertexC]);
				}
			}
			// Repeat test for face adb
			
			if (pointOutsideADB != 0)
			{
				ClosestPtPointTriangle(p, a, d, b, ref _tempResult);
				Vector3 q = _tempResult.ClosestPointOnSimplex;
				//convert result bitmask!

				float sqDist = ((Vector3)(q - p)).LengthSquared();
				if (sqDist < bestSqDist) 
				{
					bestSqDist = sqDist;
					finalResult.ClosestPointOnSimplex = q;
					finalResult.UsedVertices.UsedVertexA = _tempResult.UsedVertices.UsedVertexA;
					finalResult.UsedVertices.UsedVertexD = _tempResult.UsedVertices.UsedVertexB;
					finalResult.UsedVertices.UsedVertexB = _tempResult.UsedVertices.UsedVertexC;
					finalResult.UsedVertices.UsedVertexC = false;
					finalResult.SetBarycentricCoordinates(
							_tempResult.BarycentricCoords[VertexA],
							_tempResult.BarycentricCoords[VertexC],
							0,
							_tempResult.BarycentricCoords[VertexB]);

				}
			}
			// Repeat test for face bdc

			if (pointOutsideBDC != 0)
			{
				ClosestPtPointTriangle(p, b, d, c, ref _tempResult);
				Vector3 q = _tempResult.ClosestPointOnSimplex;
				//convert result bitmask!
				float sqDist = ((Vector3)(q - p)).LengthSquared();
				if (sqDist < bestSqDist) 
				{
					bestSqDist = sqDist;
					finalResult.ClosestPointOnSimplex = q;
					finalResult.UsedVertices.UsedVertexB = _tempResult.UsedVertices.UsedVertexA;
					finalResult.UsedVertices.UsedVertexD = _tempResult.UsedVertices.UsedVertexB;
					finalResult.UsedVertices.UsedVertexC = _tempResult.UsedVertices.UsedVertexC;
					finalResult.UsedVertices.UsedVertexA = false;
					finalResult.SetBarycentricCoordinates(
							0,
							_tempResult.BarycentricCoords[VertexA],
							_tempResult.BarycentricCoords[VertexC],
							_tempResult.BarycentricCoords[VertexB]);
				}
			}

			//help! we ended up full !

			if (finalResult.UsedVertices.UsedVertexA &&
				finalResult.UsedVertices.UsedVertexB &&
				finalResult.UsedVertices.UsedVertexC &&
				finalResult.UsedVertices.UsedVertexD) 
			{
				return true;
			}

			return true;
		}
    }
}
